String LOG_LEVEL_MSG[]={"Debug", "Info", "Attention", "Warning", "Critical", "Fatal"};
#define PS2_DAT     12
#define PS2_CMD     11
#define PS2_SEL     10
#define PS2_CLK     13

//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

volatile int log_level;
volatile int interval;
volatile int Motor_Limit_Speed;
volatile int Arm_Limit_Angle;
volatile int I_PSS_LX;
volatile int I_PSS_LY;
volatile int I_PSS_RX;
volatile int I_PSS_RY;
volatile int PSS_Target;
volatile int Arm_Node;
volatile int Arm_Target[] = {0,0,0,0};
volatile int Arm_Step;
const float Eula = 2.718281;
int motor_target[]={0, 0, 0, 0};
int Arm_Lock = false;

//Global Init
#include "Emakefun_MotorDriver.h"
#include <Wire.h>
#include "PS2X_lib.h"
#include <avr/wdt.h>
Emakefun_MotorDriver mMotorDriver = Emakefun_MotorDriver();
Emakefun_DCMotor *DCMotor_1 = mMotorDriver.getMotor(M1);
Emakefun_DCMotor *DCMotor_2 = mMotorDriver.getMotor(M2);
Emakefun_DCMotor *DCMotor_3 = mMotorDriver.getMotor(M3);
Emakefun_DCMotor *DCMotor_4 = mMotorDriver.getMotor(M4);
// Emakefun_Servo *Servo0 = mMotorDriver.getServo(2);
// Emakefun_Servo *Servo1 = mMotorDriver.getServo(3);
// Emakefun_Servo *Servo2 = mMotorDriver.getServo(4);
// Emakefun_Servo *ServoH = mMotorDriver.getServo(1);
Emakefun_Servo *Servo[4];

PS2X ps2x;

// void SerialLog(String message, int level) {
//   if (level >= log_level) {
//     Serial.println(String(String("[Runtime ") + String(millis()) + String("ms]")) + String(String("[") + String(LOG_LEVEL_MSG[level]) + String("] ")) + String(message));
//   }
// }

inline void SerialLog(String message, int level) {
  return;
}

int PSS_Input_Process(int PSS_Input) {
  PSS_Input = PSS_Input - 127;
  if (PSS_Input >= -2 && PSS_Input <= 2) {
    return 0;
  }
  return PSS_Input;
}

int Motor_Limit_Speed_Control(int motor_target_speed) {
  if (motor_target_speed > 0) {
    return min(motor_target_speed, Motor_Limit_Speed);
  } else if (motor_target_speed < 0) {
    return max(motor_target_speed, Motor_Limit_Speed * -1);
  }
  return 0;
}

inline float Speed_Curve_Ref(float input, float target, float pow_num){
  return (input/abs(input)) * abs(pow(input,pow_num) / pow(target, pow_num - 1));
}

void Calc_Translate() {
  if (I_PSS_LY) {
    PSS_Target = Speed_Curve_Ref((I_PSS_LY * Motor_Limit_Speed) / 128, Motor_Limit_Speed, 2);
    motor_target[0] = motor_target[0] + PSS_Target;
    motor_target[1] = motor_target[1] + PSS_Target;
    motor_target[2] = motor_target[2] + PSS_Target;
    motor_target[3] = motor_target[3] + PSS_Target;
    SerialLog(String("LY PSS_Target = ") + String(PSS_Target), 0);
  }
  if (I_PSS_LX) {
    PSS_Target = Speed_Curve_Ref((I_PSS_LX * Motor_Limit_Speed) / 128, Motor_Limit_Speed, 2);
    motor_target[0] = motor_target[0] + -1 * PSS_Target;
    motor_target[1] = motor_target[1] + PSS_Target;
    motor_target[2] = motor_target[2] + PSS_Target;
    motor_target[3] = motor_target[3] + -1 * PSS_Target;
    SerialLog(String("LX PSS_Target = ") + String(PSS_Target), 0);
  }
}

void Macro_Arm_AutoPlace(){
  Servo[1]->writeServo(135);
  Servo[2]->writeServo(0);
}
// 1 - 135, 2 - 0
// 145  10

bool counter_PSB_CIRCLE = false;
bool counter_PSB_SQUARE = false;

void Read_PS() {
  ps2x.read_gamepad();
  I_PSS_LX = PSS_Input_Process(ps2x.Analog(PSS_LX));
  I_PSS_LY = PSS_Input_Process(ps2x.Analog(PSS_LY));
  I_PSS_RX = PSS_Input_Process(ps2x.Analog(PSS_RX));
  I_PSS_RY = PSS_Input_Process(ps2x.Analog(PSS_RY));

  // if (ps2x.Button(PSB_PAD_UP)){
  //   Arm_Target[2] = Arm_Step;
  // } else if (ps2x.Button(PSB_PAD_DOWN)) {
  //   Arm_Target[2] = -1 * Arm_Step;
  // } else {
  //   Arm_Target[2] = 0;
  // }
  if(ps2x.Button(PSB_L1)){
    Arm_Target[0] = Arm_Step;
  } else if (ps2x.Button(PSB_L2)){
    Arm_Target[0] = -1 * Arm_Step;
  } else {
    Arm_Target[0] = 0;
  }

  if(ps2x.Button(PSB_R1)){
    Arm_Target[1] = -1 * Arm_Step;
  } else if (ps2x.Button(PSB_R2)){
    Arm_Target[1] = Arm_Step;
  } else {
    Arm_Target[1] = 0;
  }

  if(ps2x.Button(PSB_TRIANGLE)){
    Arm_Target[2] = -1 * Arm_Step;
  } else if (ps2x.Button(PSB_CROSS)){
    Arm_Target[2] = Arm_Step;
  } else {
    Arm_Target[2] = 0;
  }

  if(ps2x.Button(PSB_PAD_LEFT) && Arm_Step > 1){
    Arm_Step -= 1;
  } else if(ps2x.Button(PSB_PAD_RIGHT) && Arm_Step < 10){
    Arm_Step += 1;
  }

  if(ps2x.Button(PSB_SQUARE)){    //Deprecated
    if(Arm_Lock != 0){
      Arm_Lock = 0;
    }
    else {
      Arm_Lock = Servo[0]->readDegrees() + Arm_Target[0];
    }
  }

  if(ps2x.Button(PSB_CIRCLE)){
    if(!counter_PSB_CIRCLE){
      counter_PSB_CIRCLE = true;
      Macro_Arm_AutoPlace();
    }
  }
  else{
    counter_PSB_CIRCLE = false;
  }

  SerialLog(String("PSS_LX = ") + String(I_PSS_LX), 0);
  SerialLog(String("PSS_LY = ") + String(I_PSS_LY), 0);
  SerialLog(String("PSS_RX = ") + String(I_PSS_RX), 0);
  SerialLog(String("PSS_RY = ") + String(I_PSS_RY), 0);
  SerialLog(String("Arm_Node = ") + String(Arm_Node), 1);
}

void Apply_Motor() {
  motor_target[0] = -1 * Motor_Limit_Speed_Control(motor_target[0]);
  motor_target[1] = Motor_Limit_Speed_Control(motor_target[1]);
  motor_target[2] = -1 * Motor_Limit_Speed_Control(motor_target[2]);
  motor_target[3] = Motor_Limit_Speed_Control(motor_target[3]);
  if (motor_target[0] == 0) {
    DCMotor_1->setSpeed(0);
    DCMotor_1->run(BRAKE);
  } else if (motor_target[0] < 0) {
    DCMotor_1->setSpeed(-1 * motor_target[0]);
    DCMotor_1->run(FORWARD);
    SerialLog("DCMotor_1 run Forward", 0);
  } else {
    DCMotor_1->setSpeed(motor_target[0]);
    DCMotor_1->run(BACKWARD);
  }
  if (motor_target[1] == 0) {
    DCMotor_2->setSpeed(0);
    DCMotor_2->run(BRAKE);
  } else if (motor_target[1] < 0) {
    DCMotor_2->setSpeed(-1 * motor_target[1]);
    DCMotor_2->run(FORWARD);
  } else {
    DCMotor_2->setSpeed(abs(motor_target[1]));
    DCMotor_2->run(BACKWARD);
  }
  if (motor_target[2] == 0) {
    DCMotor_3->setSpeed(0);
    DCMotor_3->run(BRAKE);
  } else if (motor_target[2] < 0) {
    DCMotor_3->setSpeed(-1 * motor_target[2]);
    DCMotor_3->run(FORWARD);
  } else {
    DCMotor_3->setSpeed(abs(motor_target[2]));
    DCMotor_3->run(BACKWARD);
  }
  if (motor_target[3] == 0) {
    DCMotor_4->setSpeed(0);
    DCMotor_4->run(BRAKE);
  } else if (motor_target[3] < 0) {
    DCMotor_4->setSpeed(-1 * motor_target[3]);
    DCMotor_4->run(FORWARD);
  } else {
    DCMotor_4->setSpeed(abs(motor_target[3]));
    DCMotor_4->run(BACKWARD);
  }
  Serial.println("--------------------------");
  for (int i = 0; i <= 3; i = i + (1)) {
    SerialLog(String("motor_target_") + String(i) + String(" = ") + String(motor_target[i]), 0);
  }
}

void Apply_Arm() {
  Arm_Target[0] = Arm_Target[0] / 2;
  for(int i = 0 ; i < 4 ; i++)
    Servo[i]->writeServo((Arm_Limit_Angle_Control(Servo[i]->readDegrees() + Arm_Target[i])));
  
  // if(Arm_Lock != 0)            //Deprecated
  //   Servo[0]->writeServo(Arm_Lock);
}


int Arm_Limit_Angle_Control(int Arm_Target_Angle) {
  if (Arm_Target_Angle > 0) {
    return min(Arm_Target_Angle, Arm_Limit_Angle);
  }
  return 0;
}

inline float Speed_Curve_Eula_Ref(float input, float target, float pow_num){
  return (input/abs(input)) * abs(pow(Eula, 2 * ((input / target) - 2)) - 0.1353352) * target;
}

void Calc_Rotate() {
  if (I_PSS_RX) {
    PSS_Target = Speed_Curve_Ref((I_PSS_RX * Motor_Limit_Speed) / (128 * 2), Motor_Limit_Speed / 2, 2);
    motor_target[0] = motor_target[0] + -1 * PSS_Target;
    motor_target[1] = motor_target[1] + PSS_Target;
    motor_target[2] = motor_target[2] + -1 * PSS_Target;
    motor_target[3] = motor_target[3] + PSS_Target;
    SerialLog(String("RX PSS_Target = ") + String(PSS_Target), 1);
  }
}

void Reset_Motor_Target() {
  for (int i = 0; i <= 3; i = i + (1)) {
    motor_target[i] = 0;
    Arm_Target[i] = 0;
  }
}

void setup(){
  log_level = 2;
  interval = 1;
  Motor_Limit_Speed = 150;
  Arm_Limit_Angle = 180;
  I_PSS_LX = 0;
  I_PSS_LY = 0;
  I_PSS_RX = 0;
  I_PSS_RY = 0;
  PSS_Target = 0;
  Arm_Node = 0;
  Arm_Step = 5;
  Serial.begin(9600);
  mMotorDriver.begin(50);

  int error = 0;
  do{
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
    if(error == 0){
      break;
    }else{
      delay(100);
    }
  }while(1); 
  for(size_t i = 0; i < 50; i++)
  {
        ps2x.read_gamepad(false, 0);
        delay(10);
  }

  wdt_enable(WDTO_2S);
  SerialLog("UNO Init.", 1);
  ps2x.read_gamepad(false, 0);
  delay(3);
  ps2x.read_gamepad(true, 200);
  delay(300);
  ps2x.read_gamepad(false, 0);
  SerialLog("Wireless Controller connected.", 1);
  Servo[0] = mMotorDriver.getServo(1);
  Servo[1] = mMotorDriver.getServo(2);
  Servo[2] = mMotorDriver.getServo(3);
  Servo[3] = mMotorDriver.getServo(4);

  Servo[0]->writeServo(90);
  Servo[1]->writeServo(90);
  Servo[2]->writeServo(0);
  Servo[3]->writeServo(90);
  wdt_reset();
}

void loop(){
  //Loop
  SerialLog("Main Loop Reset.", 0);
  wdt_reset();
  delay(interval);
  Serial.println("\n--------------------------");
  Reset_Motor_Target();
  Read_PS();
  Calc_Translate();
  Calc_Rotate();
  Apply_Motor();
  Apply_Arm();
  // Express_Log();
}

void Express_Log(){
  SerialLog("Servo 0 = " + String(Servo[0]->readDegrees()), 2);
  SerialLog("Servo 1 = " + String(Servo[1]->readDegrees()), 2);
  SerialLog("Servo 2 = " + String(Servo[2]->readDegrees()), 2);
  SerialLog("Servo 3 = " + String(Servo[3]->readDegrees()), 2);
}
